/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    BalanceController.cpp
  * @author  Mentos Seetoo
  * @note    THe following third-party libraries are used:
  *          - Eigen3
  * @warning 
  *     - C++11 is required.												 
  ******************************************************************************
  */
#ifndef _BALANCE_CTRL_H
#define _BALANCE_CTRL_H
/* Includes ------------------------------------------------------------------*/
#include "Dynamics/dynamicModel.h"
#include "Utilities/additionTypes.h"
#include "Utilities/pid.h"

namespace Quadruped
{
template <typename T>
class BalanceController
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  
    Vec3<T> p_d, v_d, w_d;              //!< Target value
    Vec3<T> p_c, v_c, w_c;              //!< Current value
    Mat3<T> R_B, R_d;                   //!< Rotation matrix of {B} and {Desired} in {M}
    T       Kp_x, Kp_y, Kp_z,           //!< P Gain & D Gain for position loop
            Kd_x, Kd_y, Kd_z,           //!< and attitude loop.
            Kp_roll, Kp_pitch, Kp_yaw,  
            Kd_roll, Kd_pitch, Kd_yaw;  
    Vec3<T> p_error, pdot_error, ori_error, w_error;
    Vec3<T> out_v, out_w;             //!< Output of pisition loop and attitude loop.
    void SetTargets(const Vec3<T> &p, const Vec3<T> &v, const Vec3<T> &w, const Mat3<T> &R);
    void UpdateValues(const Vec3<T> &p, const Vec3<T> &v, const Vec3<T> &w, const Mat3<T> &R);
    void CalConrtollerOutput();
    void SetPGains(T x, T y, T z, T roll, T pitch, T yaw);
    void SetDGains(T x, T y, T z, T roll, T pitch, T yaw);
    void matrixLogRot(const Mat3<T>& R, Vec3<T>& omega);
    void crossMatrix(Mat3<T>& R, const Vec3<T>& omega);
    BalanceController();
private: 
    
};


} // namespace Quadruped
#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/